/**
  ******************************************************************************
  * @file           ChassisMotion.c
  * @author         竹之武; WHY
  * @date           2022.07.30
  * @brief          舵轮底盘运动学，请完善相关函数
  ******************************************************************************
  * @attent  
  * @myNote  
  ******************************************************************************
  */
#include "HeadFiles.h" 
#include "ChassisMotion.h"
/*
* @note 本文件假设所有必要的库都已经调用，你可以直接使用如下函数： 
* sin函数： arm_sin_f32(); //传入参数采用弧度制
* cos函数： arm_cos_f32();
*/ 

/*全局变量*/
extern Robot_t Robot;
/*
	Robot 是一个结构体变量，假设其中存放了机器人当前相对于世界坐标系的位置、姿态信息，由外部函数更新，相关变量如下：
	Robot.X; //float 类型
	Robot.Y; //float 类型
	Robot.Theta; //float 类型，表示车体坐标系相对于世界坐标系的旋转角度（弧度制），俯视逆时针为正，范围在(-PI,PI]之间 。
	根据任务要求选取并使用上述变量。
*/


//	p.s.如果觉得有必要可以在此处声明所需变量。 


/*全局函数*/
void vChassisMotionCalculation(float *Vel, float *Dir, float *Omega);

/*私有函数*/ 
static void prvSetMotorSpeed(float *MotorSpeed);
static void prvSetMotorDirection(float *MotorDir);


/** 
 *@brief 	本函数负责将世界坐标系下的速度信息解算到三个轮子上
 *@note 	在整个工程文件的其他代码中，这个函数会被调用以实现运动学解算。
 *@param  Vel					    : 世界坐标系下期望速度大小	[mm/s]
					   Dir					    : 世界坐标系下期望速度方向，弧度制，范围为(-PI，PI]
					   Omega				: 世界坐标系下期望角速度大小(方向为俯视逆时针为正)	[rad/s]
 */
void vChassisMotionCalculation(float *Vel, float *Dir, float *Omega);
{
	float MotorSpeed[3] = {0,0,0}; //三个驱动轮的转速，下标为0：前轮 1：左后轮 2：右后轮
    float MotorDir[3] = {0,0,0}; //三个舵轮的方向，采用弧度制，x轴正方向为0rad，范围在(-PI到PI]之间，编号同上。
	float Diff_Dir[3] = {0,0,0};

    /*
		Write your code here.
        p.s.1: 注意车体坐标系和世界坐标系之间的坐标变换。
        p.s.2: 注意车体做刚体平面运动时候的速度与角速度的叠加，你可以分别计算并将其累加在一起。
	*/
    
	prvSetMotorSpeed(MotorSpeed);   //调用私有函数，传入解算后三个驱动轮的转速[rad/s]。 
    prvSetMotorDirection(MotorDir);     //调用私有函数，传入解算后三个轮子的方向[rad]
}



/** 
 *@brief 将传入的数组中的三个驱动轮的转速发送给电机驱动器并实现闭环控制
 *@param MotorSpeed			：经过解算的三个驱动轮各自速度的数组首地址
*/
void prvSetMotorSpeed(float *MotorSpeed)
{
	/*假设这个函数由其他人实现了,你不需要在此处编写任何代码*/
} 


/**
 *@brief 将传入的数组中三个舵轮的方向换算为舵向电机的转动圈数并实现闭环控制
 *@param MotorDir                   : 经过解算的三个舵轮的朝向数组首地址
 */
static void prvSetMotorDirection(float *MotorDir)
{
    /*假设这个函数由其他人实现了,你不需要在此处编写任何代码*/
}
